//
// Created by LiuDongPeng on 2023/11/16.
//

#include "lowpass_filter.h"




int low_pass_filter_init(low_pass_filter_t* filter, float fc, float dt)
{
	if (filter == NULL)
		return -1;

	filter->fc = fc;
	filter->dt = dt;

	float tmp = M_TWOPI * fc * dt;
	filter->alpha = tmp / (tmp + 1.0f);

	return 0;
}

float low_pass_filter_work(low_pass_filter_t* filter, float val)
{
	float out;

	static int firstHere = 1;
	if (firstHere)
	{
		filter->lastVal = val;
		firstHere = 0;
	}

	out = filter->lastVal + filter->alpha * (val - filter->lastVal);
	filter->lastVal = out;

	return out;
}

float lowpass(float dt, float endFreq, float data)
{
//	static float lastData;
//
//	float alpha = dt / (endFreq * 2);
//
//	return (data + alpha * (data - lastData)) / (1 + alpha);

	static float lastData = 0.0f;

	float result = 0.9f * lastData + 0.1f * data;
	lastData = result;

	return result;
}
